﻿using System.Text;

namespace TPXSDK.NET.Mission.Instruction;

internal class InstPWMCtrl : Tokenizer
{
    public InstPWMCtrl(TokenizerAdmin admin)
    {
        Admin = admin;
        KeyName = "pwmctrl";
        KeyValue = (int)FlyCmdId.CMD_PWMCTRL;
        Usage = "pwmctrl ch pulse";
        Description = @"set servo to Pulse width in us.";
    }

    public override bool Process()
    {
        var flyCmp = Admin?.Compiler; if (null == flyCmp) return false;
        var tokens = flyCmp.GetLineTokens();

        if (null == tokens || tokens.Count != 3)
        {
            flyCmp.ErrorMsg = $"{KeyName} Invalid Usage";
            return false;
        }

        var cmd = new FlyCmdItem
        {
            Id = (byte)KeyValue,
            LineNum = tokens[0].Row
        };
        tokens.RemoveAt(0);// Remove cmd Name

        if (tokens[0].Type != TokenType.NUMBER)
        {
            flyCmp.ErrorMsg = $"{KeyName} Invalid param";
            return false;
        }

        if (tokens[1].Type != TokenType.NUMBER)
        {
            flyCmp.ErrorMsg = $"{KeyName} Invalid param";
            return false;
        }

        // 设置伺服通道
        cmd.P1 = (byte)ParaType.NUMBER;
        cmd.P3.U32Val = Convert.ToUInt32(tokens[0].Name);

        // 设置伺服脉宽
        cmd.P2 = FlyUtils.ToInt16(tokens[1].Name);

        flyCmp.CmdList.Add(cmd);

        return true;
    }

    public override void Reverse(FlyCmdItem cmd, StringBuilder sb)
    {
        if (cmd.P1 == (byte)ParaType.NUMBER)
        {
            sb.AppendLine($"{KeyName} {cmd.P3.U32Val} {cmd.P2}");
        }
        else
        {
            sb.AppendLine($"{KeyName} ?");
        }
    }
}
